#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

int main (int argc, char** argv)
{
    // std::cout << "Hello, it's my first pcl cmake project!" << std::endl;

    // 第1步： 处理参数
    int proportion;                              // 取的比例
    std::string infile = argv[1];                // pcd文件名 (带扩展名：.pcd)
    std::istringstream (argv[2]) >> proportion;   // threshold

    // 第2步： 从pcd文件读入点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCDReader reader;
    reader.read (infile, *cloud);

    // 第3步： 遍历所有点随机取或不取
    std::vector<int> inliers;
    int tot_points = cloud->width * cloud->height;
    for(int i = 0; i < tot_points; i++){
        if(rand()%proportion==0)    inliers.push_back(i);
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_compressed(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_compressed->width  = inliers.size ();
    cloud_compressed->height = 1;
    cloud_compressed->points.resize (cloud_compressed->width * cloud_compressed->height);

    for (std::size_t i = 0; i < inliers.size (); ++i)
    {
        cloud_compressed->points[i].x = cloud->points[inliers[i]].x;
        cloud_compressed->points[i].y = cloud->points[inliers[i]].y;
        cloud_compressed->points[i].z = cloud->points[inliers[i]].z;
    }

    // 第4步： 保存取部分到pcd文件
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ> ("res_compress.pcd", *cloud_compressed, false);

    return 0;
}